"""
Euler angles are in Bunge convention, i.e. ZXZ rotation
"""
import sys
import numpy as np

P = -1
"""
Define permutation index to make conversion internally consistent
Reference DOI: 10.1088/0965-0393/23/8/083501
"""


class rot(object):

    def __init__(self):
        pass

    def mat2quat(self, _rm):
        """
        Implementing Cayley's method
        Reference DOI: 10.1115/1.4041889
        """
        # ----- Check numpy data type -----
        if not type(_rm) is np.ndarray:
            _rm = np.array(_rm)
        # ---------------------------------
        _q = np.zeros([4, ])

        _q[0] = (1 / 4) * np.sqrt((_rm[0, 0] + _rm[1, 1] + _rm[2, 2] + 1) ** 2 +
                                  (_rm[2, 1] - _rm[1, 2]) ** 2 +
                                  (_rm[0, 2] - _rm[2, 0]) ** 2 +
                                  (_rm[1, 0] - _rm[0, 1]) ** 2)

        _q[1] = P * np.sign(_rm[2, 1] - _rm[1, 2]) * (1 / 4) * np.sqrt((_rm[2, 1] - _rm[1, 2]) ** 2 +
                                                                       (_rm[0, 0] - _rm[1, 1] - _rm[2, 2] + 1) ** 2 +
                                                                       (_rm[1, 0] + _rm[0, 1]) ** 2 +
                                                                       (_rm[2, 0] + _rm[0, 2]) ** 2)
        _q[2] = P * np.sign(_rm[0, 2] - _rm[2, 0]) * (1 / 4) * np.sqrt((_rm[0, 2] - _rm[2, 0]) ** 2 +
                                                                       (_rm[1, 0] + _rm[0, 1]) ** 2 +
                                                                       (_rm[1, 1] - _rm[0, 0] - _rm[2, 2] + 1) ** 2 +
                                                                       (_rm[2, 1] + _rm[1, 2]) ** 2)
        _q[3] = P * np.sign(_rm[1, 0] - _rm[0, 1]) * (1 / 4) * np.sqrt((_rm[1, 0] - _rm[0, 1]) ** 2 +
                                                                       (_rm[2, 0] + _rm[0, 2]) ** 2 +
                                                                       (_rm[2, 1] + _rm[1, 2]) ** 2 +
                                                                       (_rm[2, 2] - _rm[0, 0] - _rm[1, 1] + 1) ** 2)

        return _q

    def mat2quat2(R):
        """
        Implementing Bar-Itzhack algorithm
        If rotation matrix is not orthogonal, use this method
        Reference DOI: 10.2514/2.4654
        """
        # ----- Check numpy data type -----
        if not type(R) is np.ndarray:
            R = np.array(R)
        # ---------------------------------
        K = np.zeros([4, 4])

        K[0, 0] = 1 / 3 * (R[0, 0] - R[1, 1] - R[2, 2])
        K[1, 0] = 1 / 3 * (R[1, 0] + R[0, 1])
        K[2, 0] = 1 / 3 * (R[2, 0] + R[0, 2])
        K[3, 0] = 1 / 3 * (R[1, 2] - R[2, 1])

        K[0, 1] = 1 / 3 * (R[1, 0] + R[0, 1])
        K[1, 1] = 1 / 3 * (R[1, 1] - R[0, 0] - R[2, 2])
        K[2, 1] = 1 / 3 * (R[2, 1] + R[1, 2])
        K[3, 1] = 1 / 3 * (R[2, 0] - R[0, 2])

        K[0, 2] = 1 / 3 * (R[2, 0] + R[0, 2])
        K[1, 2] = 1 / 3 * (R[2, 1] + R[1, 2])
        K[2, 2] = 1 / 3 * (R[2, 2] - R[0, 0] - R[1, 1])
        K[3, 2] = 1 / 3 * (R[0, 1] - R[1, 0])

        K[0, 3] = 1 / 3 * (R[1, 2] - R[2, 1])
        K[1, 3] = 1 / 3 * (R[2, 0] - R[0, 2])
        K[2, 3] = 1 / 3 * (R[0, 1] - R[1, 0])
        K[3, 3] = 1 / 3 * (R[0, 0] + R[1, 1] + R[2, 2])

        eigen = np.linalg.eig(K)
        quat = eigen[1][:, eigen[0].argmax()]
        quat = np.array([quat[3], quat[0], quat[1], quat[2]])

        # Introducing permutation index
        quat[1] = -P * quat[1]
        quat[2] = -P * quat[2]
        quat[3] = -P * quat[3]

        if quat[0] < 0:
            quat = -1 * quat
        else:
            pass

        return quat

    def mat2quat3(R):

        # No need to use permutation index

        anx = rot.mat2anx(R)
        quat = rot.anx2quat(anx)

        return quat

    def quat2mat(quat):
        # ----- Check numpy data type -----
        if not type(quat) is np.ndarray:
            quat = np.array(quat)
        # ---------------------------------
        R = np.zeros([3, 3])

        # Introducing permutation index

        R[0, 0] = 1 - 2 * (quat[2] ** 2 + quat[3] ** 2)
        R[1, 0] = 2 * (P * (quat[0] * quat[3]) + quat[1] * quat[2])
        R[2, 0] = 2 * (quat[1] * quat[3] - P * (quat[0] * quat[2]))

        R[0, 1] = 2 * (quat[1] * quat[2] - P * (quat[0] * quat[3]))
        R[1, 1] = 1 - 2 * (quat[1] ** 2 + quat[3] ** 2)
        R[2, 1] = 2 * (P * (quat[0] * quat[1]) + quat[2] * quat[3])

        R[0, 2] = 2 * (P * (quat[0] * quat[2]) + quat[1] * quat[3])
        R[1, 2] = 2 * (quat[2] * quat[3] - P * (quat[0] * quat[1]))
        R[2, 2] = 1 - 2 * (quat[1] ** 2 + quat[2] ** 2)

        return R

    def euler2quat(euler):
        # ----- Check numpy data type -----
        if not type(euler) is np.ndarray:
            euler = np.array(euler)
        # ---------------------------------
        euler = euler * np.pi / 180

        phi1 = euler[0]
        cphi = euler[1]
        phi2 = euler[2]

        quat = np.zeros([4, ])

        quat[0] = np.cos(cphi / 2) * np.cos((phi1 + phi2) / 2)
        quat[1] = -P * np.sin(cphi / 2) * np.cos((phi1 - phi2) / 2)
        quat[2] = -P * np.sin(cphi / 2) * np.sin((phi1 - phi2) / 2)
        quat[3] = -P * np.cos(cphi / 2) * np.sin((phi1 + phi2) / 2)

        if quat[0] < 0:
            quat = -1 * quat
        else:
            pass

        return quat

    def quat2euler(quat):
        # ----- Check numpy data type -----
        if not type(quat) is np.ndarray:
            quat = np.array(quat)
        # ---------------------------------

        euler = np.zeros([3, ])

        # Setting conditions
        q03 = quat[0] ** 2 + quat[3] ** 2
        q12 = quat[1] ** 2 + quat[2] ** 2
        chi = np.sqrt(q03 * q12)

        if chi < 1.0e-8:
            if q12 < 1.0e-8:
                euler[0] = np.arctan2(-2 * P * quat[0] * quat[3], quat[0] ** 2 - quat[3] ** 2)
                euler[1] = 0
                euler[2] = 0
            if q03 < 1.0e-8:
                euler[0] = np.arctan2(2 * quat[1] * quat[2], quat[1] ** 2 - quat[2] ** 2)
                euler[1] = np.pi
                euler[2] = 0
        else:
            euler[0] = np.arctan2(quat[1] * quat[3] - P * (quat[0] * quat[2]),
                                  -P * (quat[0] * quat[1]) - quat[2] * quat[3])
            euler[1] = np.arctan2(2 * chi, q03 - q12)
            euler[2] = np.arctan2(quat[1] * quat[3] + P * (quat[0] * quat[2]),
                                  quat[2] * quat[3] - P * (quat[0] * quat[1]))

        for i in range(3):
            if euler[i] < 0:
                euler[i] += 2 * np.pi

        euler = euler * 180 / np.pi

        return euler

    def euler2mat(euler):
        # ----- Check numpy data type -----
        if not type(euler) is np.ndarray:
            euler = np.array(euler)
        # ---------------------------------

        euler = euler * np.pi / 180

        phi1 = euler[0]
        cphi = euler[1]
        phi2 = euler[2]
        R = np.array([[np.cos(phi1) * np.cos(phi2) - np.sin(phi1) * np.sin(phi2) * np.cos(cphi),
                       np.sin(phi1) * np.cos(phi2) + np.cos(phi1) * np.sin(phi2) * np.cos(cphi),
                       np.sin(phi2) * np.sin(cphi)],
                      [-np.cos(phi1) * np.sin(phi2) - np.sin(phi1) * np.cos(phi2) * np.cos(cphi),
                       -np.sin(phi1) * np.sin(phi2) + np.cos(phi1) * np.cos(phi2) * np.cos(cphi),
                       np.cos(phi2) * np.sin(cphi)],
                      [np.sin(phi1) * np.sin(cphi), -np.cos(phi1) * np.sin(cphi), np.cos(cphi)]])

        return R

    def mat2euler(R):
        # ----- Check numpy data type -----
        if not type(R) is np.ndarray:
            R = np.array(R)
        # ---------------------------------

        euler = np.zeros([3, ])
        if np.abs(np.abs(R[2, 2]) - 1) > 1e-8:
            euler[0] = np.arctan2(R[2, 0], -R[2, 1])
            euler[1] = np.arccos(R[2, 2])
            euler[2] = np.arctan2(R[0, 2], R[1, 2])
        else:
            euler[0] = np.arctan2(R[0, 1], R[0, 0])
            euler[1] = (np.pi / 2) * (1 - R[2, 2])
            euler[2] = 0

        if euler[0] < 0:
            euler[0] += 2 * np.pi
        if euler[2] < 0:
            euler[2] += 2 * np.pi

        euler = euler * 180 / np.pi

        # using different approach
        # quat = rot.mat2quat(R)
        # euler = rot.quat2euler(quat)

        return (euler)

    def mat2anx(R):
        # ----- Check numpy data type -----
        if not type(R) is np.ndarray:
            R = np.array(R)
        # ---------------------------------
        anx = np.zeros([4, ])
        omega = np.arccos((R[0, 0] + R[1, 1] + R[2, 2] - 1) / 2)
        # n = np.zeros([3, ])
        eigen = np.linalg.eig(R)
        for i in range(3):
            if eigen[0][i].imag == 0:
                idx = i

        if omega > 1e-8:
            n = np.abs(eigen[1][:, idx].real)
            n[0] = n[0] * np.sign(P * (R[2, 1] - R[1, 2]))
            n[1] = n[1] * np.sign(P * (R[0, 2] - R[2, 0]))
            n[2] = n[2] * np.sign(P * (R[1, 0] - R[0, 1]))
            # n[0] = R[2, 1] - R[1, 2]
            # n[1] = R[0, 2] - R[2, 0]
            # n[2] = R[1, 0] - R[0, 1]
            # n = n / (2 * np.sin(omega))
        else:
            n = np.array([0, 0, 1])

        omega = omega * 180 / np.pi

        anx[0] = omega
        anx[1:4] = n

        return anx

    def anx2quat(anx):
        # ----- Check numpy data type -----
        if not type(anx) is np.ndarray:
            anx = np.array(anx)
        # ---------------------------------

        omega = anx[0] * np.pi / 180
        n = anx[1:4]

        quat = np.zeros([4, ])

        quat[0] = np.cos(omega / 2)
        quat[1:4] = np.sin(omega / 2) * n

        if quat[0] < 0:
            quat = -1 * quat
        else:
            pass

        return quat

    def quat2anx(quat):
        # ----- Check numpy data type -----
        if not type(quat) is np.ndarray:
            quat = np.array(quat)
        # ---------------------------------

        if quat[0] < 0:
            quat = -1 * quat
        else:
            pass

        q0 = quat[0]
        q1 = quat[1]
        q2 = quat[2]
        q3 = quat[3]

        if np.abs(q0 - 1) < 1e-3:
            omega = 0
            n = np.array([0, 0, 1])
        elif np.abs(q0) < 1e-8:
            omega = np.pi
            n = np.array([q1, q2, q3])
        else:
            omega = 2 * np.arccos(q0)
            s = np.sign(q0) / np.sqrt(q1 ** 2 + q2 ** 2 + q3 ** 2)
            n = s * np.array([q1, q2, q3])

        omega = omega * 180 / np.pi

        anx = np.zeros([4, ])
        anx[0] = omega
        anx[1:4] = n
        return anx


class wabha(object):
    """
    Implementing algorithms to solve Wabha's problem:
    a.k.a. Calculate rotation matrix R between two pairs of vectors (v1, u1), (v2, u2)
    R @ v1 = v2; R @ u1 = u2
    v1, u1 cannot be parallel and zero vectors
    """

    # ---------------------------------------------------------------------------------
    def check_angle(v1, v2):
        """
        v1 is your firsr vector
        v2 is your second vector
        Inputs are column vectors
        """

        norm1 = np.linalg.norm(v1)
        norm2 = np.linalg.norm(v2)
        norm3 = np.linalg.norm(np.cross(v1, v2))
        try:
            if norm1 == 0:
                raise ValueError('First vector is zero')
            if norm2 == 0:
                raise ValueError('Second vector is zero')
            if norm3 == 0:
                raise ValueError('First and second vector are parallel')
            else:
                pass
                # print('Checking done!')
        except ValueError as e:
            print('ValueError:', e)
            sys.exit(1)

        # return None
        # angle = np.arccos((v1 @ v2) / (norm1 * norm2))
        # return angle

    def normalized_vec3d(vec):
        vec_hat = vec / np.linalg.norm(vec, axis=0)
        return vec_hat

    # ---------------------------------------------------------------------------------
    # TRIAD ALGORITHM
    def triad(vs1, vs2):
        """
        vs1 and vs2 are column vectors

        """
        # print('Checking angles between first vector pair...')
        wabha.check_angle(vs1[:, 0], vs2[:, 0])
        # print('Checking angles between second vector pair...')
        wabha.check_angle(vs1[:, 1], vs2[:, 1])

        a = wabha.normalized_vec3d(vs1)
        v = np.cross(a[:, 0], a[:, 1])
        v = v.reshape(3, 1)
        a = np.hstack((a, v))
        b = np.linalg.inv(a)

        a = wabha.normalized_vec3d(vs2)
        v = np.cross(a[:, 0], a[:, 1])
        v = v.reshape(3, 1)
        a = np.hstack((a, v))

        rot = np.zeros((3, 3))
        for j in range(3):
            for i in range(3):
                for k in range(3):
                    rot[i, j] = rot[i, j] + a[i, k] * b[k, j]
        return rot

    # ---------------------------------------------------------------------------------
    # KABASCH ALGORITHM
    def kabsch(P, Q):
        """
        Using the Kabsch algorithm with two sets of paired point P and Q, centered
        around the centroid. Each vector set is represented as an NxD
        matrix, where D is the the dimension of the space.
        The algorithm works in two steps:
        - the computation of a covariance matrix C
        - computation of the optimal rotation matrix U
        For more info see http://en.wikipedia.org/wiki/Kabsch_algorithm
        Parameters
        ----------
        P, Q are column vectors
        P : array
            (D,N) matrix, where D is dimension and N is points.
        Q : array
            (D,N) matrix, where D is dimension and N is points.
        Returns
        -------
        rot : matrix
            Rotation matrix (D,D)
        """
        print('Checking angles between first vector pair...')
        wabha.check_angle(P[:, 0], Q[:, 0])
        print('Checking angles between second vector pair...')
        wabha.check_angle(P[:, 1], Q[:, 1])

        # Computation of the covariance matrix
        # C = np.dot(np.transpose(Q), P) # Change covariance matrix if row vectors are received
        # C = np.dot(Q, np.transpose(P)) # To calculate rot @ Q = P
        C = np.dot(Q, np.transpose(P))  # To calculate rot @ P = Q

        # Computation of the optimal rotation matrix
        # This can be done using singular value decomposition (SVD)
        # Getting the sign of the det(V)*(W) to decide
        # whether we need to correct our rotation matrix to ensure a
        # right-handed coordinate system.
        # And finally calculating the optimal rotation matrix 'rot'
        # see http://en.wikipedia.org/wiki/Kabsch_algorithm
        V, S, W = np.linalg.svd(C)
        d = (np.linalg.det(V) * np.linalg.det(W)) < 0.0

        if d:
            S[-1] = -S[-1]
            V[:, -1] = -V[:, -1]

        # Create Rotation matrix 'rot'
        rot = np.dot(V, W)

        return rot

    # ---------------------------------------------------------------------------------
    # DUAL QUATERNION ALGORITHM
    def quaternion_transform(r):
        """
        Get optimal rotation
        """
        Wt_r = wabha.makeW(*r).T
        Q_r = wabha.makeQ(*r)
        rot = Wt_r.dot(Q_r)[:3, :3]
        return rot

    def makeW(r1, r2, r3, r4=0):
        """
        matrix involved in quaternion rotation
        """
        W = np.asarray([
            [r4, r3, -r2, r1],
            [-r3, r4, r1, r2],
            [r2, -r1, r4, r3],
            [-r1, -r2, -r3, r4]])
        return W

    def makeQ(r1, r2, r3, r4=0):
        """
        matrix involved in quaternion rotation
        """
        Q = np.asarray([
            [r4, -r3, r2, r1],
            [r3, r4, -r1, r2],
            [-r2, r1, r4, r3],
            [-r1, -r2, -r3, r4]])
        return Q

    def quaternion_rotate(Y, X):
        """
        Calculate the rotation

        Parameters
        ----------
        X : array
            (N,D) matrix, where N is points and D is dimension.
        Y: array
            (N,D) matrix, where N is points and D is dimension.

        Returns
        -------
        rot : matrix
            Rotation matrix (D,D)
        """
        X = X.T
        Y = Y.T

        print('Checking angles between first vector pair...')
        wabha.check_angle(X[:, 0], Y[:, 0])
        print('Checking angles between second vector pair...')
        wabha.check_angle(X[:, 1], Y[:, 1])

        N = X.shape[0]
        W = np.asarray([wabha.makeW(*Y[k]) for k in range(N)])
        Q = np.asarray([wabha.makeQ(*X[k]) for k in range(N)])
        Qt_dot_W = np.asarray([np.dot(Q[k].T, W[k]) for k in range(N)])
        # NOTE UNUSED W_minus_Q = np.asarray([W[k] - Q[k] for k in range(N)])
        A = np.sum(Qt_dot_W, axis=0)
        eigen = np.linalg.eigh(A)
        r = eigen[1][:, eigen[0].argmax()]
        rot = wabha.quaternion_transform(r)
        return rot


# ---------------------------------------------------------------------------------
class quaternion(object):

    def dot(q1, q2):

        d = np.zeros([4, ])
        d[0] = q1[0] * q2[0]
        d[1] = q1[1] * q2[1]
        d[2] = q1[2] * q2[2]
        d[3] = q1[3] * q2[3]

        return d

    def times(q1, q2):
        [a1, b1, c1, d1] = q1
        [a2, b2, c2, d2] = q2

        quat = np.zeros([4, ])
        quat[0] = a1 * a2 - b1 * b2 - c1 * c2 - d1 * d2
        quat[1] = b1 * a2 + a1 * b2 - d1 * c2 + c1 * d2
        quat[2] = c1 * a2 + d1 * b2 + a1 * c2 - b1 * d2
        quat[3] = d1 * a2 - c1 * b2 + b1 * c2 + a1 * d2

        return quat

    def outer(g1, g2):
        if not type(g1 or g2) is np.ndarray:
            g1, g2 = np.array(g1), np.array(g2)

        d = g1 * np.transpose(g2)

        return d

    def conj(quat):
        if not type(quat) is np.ndarray:
            quat = np.array(quat)

        q_conj = np.zeros([4, ])
        q_conj[0] = quat[0]
        q_conj[1] = -1 * quat[1]
        q_conj[2] = -1 * quat[2]
        q_conj[3] = -1 * quat[3]

        return q_conj

    def rand(n=1):
        """
        Creating random quaternions uniformly
        Reference DOI: 10.1016/B978-0-08-050755-2.50036-1

        Generated quaternions are stored as column vectors
        """
        quat = np.zeros((4, n))
        for i in range(n):
            u = np.random.random()
            v = np.random.random()
            w = np.random.random()
            quat[0, i] = np.sqrt(1 - u) * np.sin(2 * np.pi * v)
            quat[1, i] = np.sqrt(1 - u) * np.cos(2 * np.pi * v)
            quat[2, i] = np.sqrt(u) * np.sin(2 * np.pi * w)
            quat[3, i] = np.sqrt(u) * np.cos(2 * np.pi * w)
        return quat

    def to_quat(v):
        """
        Make vector or vector sets to quaternions
        q = a + bi + cj + dk
        INPUT: v(3, N), N is number of vectors.
        """
        v = np.insert(v, 0, 0, axis=0)
        return v

    def lhs(quat):
        if not type(quat) is np.ndarray:
            quat = np.array(quat)

        [a, b, c, d] = quat

        lhsm = np.zeros([4, 4])
        lhsm[:, 0] = [a, b, c, d]
        lhsm[:, 1] = [-b, a, d, -c]
        lhsm[:, 2] = [-c, -d, a, b]
        lhsm[:, 3] = [-d, c, -b, a]

        return lhsm

    def rhs(quat):
        if not type(quat) is np.ndarray:
            quat = np.array(quat)

        [a, b, c, d] = quat

        rhsm = np.zeros([4, 4])
        rhsm[:, 0] = [a, b, c, d]
        rhsm[:, 1] = [-b, a, -d, c]
        rhsm[:, 2] = [-c, d, a, -b]
        rhsm[:, 3] = [-d, -c, b, a]

        return rhsm

    def rotv(quat, v):
        """
        This is less efficient to function ROTV, can be used to cross-verify
        :param v: v can be a 3d vector or quaternion
        :return: v as column vector
        """
        if not type(quat) is np.ndarray:
            quat = np.array(quat)

        rota = rot.quat2mat(quat)
        v = rota @ v

        return v

    def rotv2(quat, v):
        """
        This is less efficient to function ROTV, can be used to cross-verify
        :param v: v can be a 3d vector or quaternion
        :return: v as column vector
        """
        if not type(quat) is np.ndarray:
            quat = np.array(quat)

        if P == -1:
            quat = quaternion.conj(quat)
        else:
            pass

        v = v.reshape([3, -1])
        v = quaternion.to_quat(v)
        # This is active rotation
        v = quaternion.rhs(quaternion.conj(quat)) @ quaternion.lhs(quat) @ v
        v = v[1: 4]
        # rota = quaternion.rhs(quaternion.conj(quat)) @ quaternion.lhs(quat)
        # rota = rota[1:4, 1:4]
        # v = rota @ v

        return v

    def rotq(quat, q):

        q = quaternion.rhs(quaternion.conj(quat)) @ quaternion.lhs(quat) @ q

        return q

    def fnorm(quat):
        norm = np.linalg.norm(quat, axis=0)
        return norm


class mori(object):
    def __init__(self, quat1, quat2):
        self.quat1 = quat1
        self.quat2 = quat2
        self.quatSym1 = np.array([])
        self.quatSym2 = np.array([])
        self.numEqt = 0

    @property
    def cubic(self):
        self.quatSym1 = mori.cubicSymmetries(self.quat1)
        self.quatSym2 = mori.cubicSymmetries(self.quat2)
        self.numEqt = 24

    def cubicSymmetries(inQuat):
        q0 = inQuat[0]
        q1 = inQuat[1]
        q2 = inQuat[2]
        q3 = inQuat[3]
        quatSym = np.zeros([4, 24])
        quatSym[:, 0] = np.array([q0, q1, q2, q3])
        quatSym[:, 1] = np.array([q3, q2, -q1, -q0])
        quatSym[:, 2] = np.array([-q2, q3, q0, -q1])
        quatSym[:, 3] = np.array([q1, -q0, q3, -q2])
        quatSym[:, 4] = np.array(
            [(q0 + q3 + q1 - q2) / 2, (q1 + q3 + q2 - q0) / 2, (q2 + q3 + q0 - q1) / 2, (q3 - q0 - q1 - q2) / 2])
        quatSym[:, 5] = np.array(
            [(q0 - q3 - q1 + q2) / 2, (q1 - q3 - q2 + q0) / 2, (q2 - q3 - q0 + q1) / 2, (q3 + q0 + q1 + q2) / 2])
        quatSym[:, 6] = np.array(
            [(q0 - q3 + q1 - q2) / 2, (q1 + q3 - q2 - q0) / 2, (q2 + q3 + q0 + q1) / 2, (q3 + q0 - q1 - q2) / 2])
        quatSym[:, 7] = np.array(
            [(q0 + q3 - q1 + q2) / 2, (q1 - q3 + q2 + q0) / 2, (q2 - q3 - q0 - q1) / 2, (q3 - q0 + q1 + q2) / 2])
        quatSym[:, 8] = np.array(
            [(q0 + q3 + q1 + q2) / 2, (q1 - q3 + q2 - q0) / 2, (q2 + q3 - q0 - q1) / 2, (q3 - q0 + q1 - q2) / 2])
        quatSym[:, 9] = np.array(
            [(q0 - q3 - q1 - q2) / 2, (q1 + q3 - q2 + q0) / 2, (q2 - q3 + q0 + q1) / 2, (q3 + q0 - q1 + q2) / 2])
        quatSym[:, 10] = np.array(
            [(q0 + q3 - q1 - q2) / 2, (q1 + q3 + q2 + q0) / 2, (q2 - q3 + q0 - q1) / 2, (q3 - q0 - q1 + q2) / 2])
        quatSym[:, 11] = np.array(
            [(q0 - q3 + q1 + q2) / 2, (q1 - q3 - q2 - q0) / 2, (q2 + q3 - q0 + q1) / 2, (q3 + q0 + q1 - q2) / 2])
        quatSym[:, 12] = np.array(
            [(q1 - q2) / np.sqrt(2), (q3 - q0) / np.sqrt(2), (q3 + q0) / np.sqrt(2), (-q1 - q2) / np.sqrt(2)])
        quatSym[:, 13] = np.array(
            [(q3 + q1) / np.sqrt(2), (q2 - q0) / np.sqrt(2), (q3 - q1) / np.sqrt(2), (-q0 - q2) / np.sqrt(2)])
        quatSym[:, 14] = np.array(
            [(q3 - q2) / np.sqrt(2), (q3 + q2) / np.sqrt(2), (q0 - q1) / np.sqrt(2), (-q0 - q1) / np.sqrt(2)])
        quatSym[:, 15] = np.array(
            [(-q1 - q2) / np.sqrt(2), (q3 + q0) / np.sqrt(2), (-q3 + q0) / np.sqrt(2), (-q1 + q2) / np.sqrt(2)])
        quatSym[:, 16] = np.array(
            [(q3 - q1) / np.sqrt(2), (q2 + q0) / np.sqrt(2), (-q3 - q1) / np.sqrt(2), (-q0 + q2) / np.sqrt(2)])
        quatSym[:, 17] = np.array(
            [(q3 + q2) / np.sqrt(2), (-q3 + q2) / np.sqrt(2), (-q0 - q1) / np.sqrt(2), (-q0 + q1) / np.sqrt(2)])
        quatSym[:, 18] = np.array(
            [(q0 + q3) / np.sqrt(2), (q1 + q2) / np.sqrt(2), (q2 - q1) / np.sqrt(2), (q3 - q0) / np.sqrt(2)])
        quatSym[:, 19] = np.array(
            [(q0 - q2) / np.sqrt(2), (q1 + q3) / np.sqrt(2), (q2 + q0) / np.sqrt(2), (q3 - q1) / np.sqrt(2)])
        quatSym[:, 20] = np.array(
            [(q0 + q1) / np.sqrt(2), (q1 - q0) / np.sqrt(2), (q2 + q3) / np.sqrt(2), (q3 - q2) / np.sqrt(2)])
        quatSym[:, 21] = np.array(
            [(q0 - q3) / np.sqrt(2), (q1 - q2) / np.sqrt(2), (q2 + q1) / np.sqrt(2), (q3 + q0) / np.sqrt(2)])
        quatSym[:, 22] = np.array(
            [(q0 + q2) / np.sqrt(2), (q1 - q3) / np.sqrt(2), (q2 - q0) / np.sqrt(2), (q3 + q1) / np.sqrt(2)])
        quatSym[:, 23] = np.array(
            [(q0 - q1) / np.sqrt(2), (q1 + q0) / np.sqrt(2), (q2 - q3) / np.sqrt(2), (q3 + q2) / np.sqrt(2)])
        return quatSym

    @property
    def calcMoriAngle(self):
        for i in range(self.numEqt):
            for j in range(self.numEqt):
                tempA = self.quatSym1[:, i]
                tempB = self.quatSym2[:, j]
                tempQ = quaternion.times(tempB, quaternion.conj(tempA))
                angleAxis = rot.quat2anx(tempQ)
                if i + j == 0:
                    angle = angleAxis[0]
                    axis = angleAxis[1:4]
                else:
                    if angle < angleAxis[0]:
                        pass
                    else:
                        angle = angleAxis[0]
                        axis = angleAxis[1:4]
                j += 1
            i += 1

        for i in range(self.numEqt):
            for j in range(self.numEqt):
                tempA = self.quatSym2[:, i]
                tempB = self.quatSym1[:, j]
                tempQ = quaternion.times(tempB, quaternion.conj(tempA))
                angleAxis = rot.quat2anx(tempQ)
                if angle < angleAxis[0]:
                    pass
                else:
                    angle = angleAxis[0]
                    axis = angleAxis[1:4]
                j += 1
            i = + 1

        angleAxis = np.zeros([4, ])
        angleAxis[0] = angle
        angleAxis[1:4] = axis

        return angleAxis


class symOperator(object):

    def __init__(self):
        self.symOpCubic = np.zeros([4, 24])
        self.symOpHex = np.zeros([4, 12])

    @property
    def cubic(self):
        self.symOpCubic[:, 0] = np.array([1, 0, 0, 0])
        self.symOpCubic[:, 1] = np.array([0, 1, 0, 0])
        self.symOpCubic[:, 2] = np.array([0, 0, 1, 0])
        self.symOpCubic[:, 3] = np.array([0, 0, 0, 1])
        self.symOpCubic[:, 4] = np.array([0.5, 0.5, 0.5, 0.5])
        self.symOpCubic[:, 5] = np.array([0.5, -0.5, -0.5, -0.5])
        self.symOpCubic[:, 6] = np.array([0.5, 0.5, -0.5, 0.5])
        self.symOpCubic[:, 7] = np.array([0.5, -0.5, 0.5, -0.5])
        self.symOpCubic[:, 8] = np.array([0.5, -0.5, 0.5, 0.5])
        self.symOpCubic[:, 9] = np.array([0.5, 0.5, -0.5, -0.5])
        self.symOpCubic[:, 10] = np.array([0.5, -0.5, -0.5, 0.5])
        self.symOpCubic[:, 11] = np.array([0.5, 0.5, 0.5, -0.5])
        self.symOpCubic[:, 12] = np.array([1 / np.sqrt(2), 1 / np.sqrt(2), 0, 0])
        self.symOpCubic[:, 13] = np.array([1 / np.sqrt(2), 0, 1 / np.sqrt(2), 0])
        self.symOpCubic[:, 14] = np.array([1 / np.sqrt(2), 0, 0, 1 / np.sqrt(2)])
        self.symOpCubic[:, 15] = np.array([1 / np.sqrt(2), -1 / np.sqrt(2), 0, 0])
        self.symOpCubic[:, 16] = np.array([1 / np.sqrt(2), 0, -1 / np.sqrt(2), 0])
        self.symOpCubic[:, 17] = np.array([1 / np.sqrt(2), 0, 0, -1 / np.sqrt(2)])
        self.symOpCubic[:, 18] = np.array([0, 1 / np.sqrt(2), 1 / np.sqrt(2), 0])
        self.symOpCubic[:, 19] = np.array([0, -1 / np.sqrt(2), 1 / np.sqrt(2), 0])
        self.symOpCubic[:, 20] = np.array([0, 0, 1 / np.sqrt(2), 1 / np.sqrt(2)])
        self.symOpCubic[:, 21] = np.array([0, 0, -1 / np.sqrt(2), 1 / np.sqrt(2)])
        self.symOpCubic[:, 22] = np.array([0, 1 / np.sqrt(2), 0, 1 / np.sqrt(2)])
        self.symOpCubic[:, 23] = np.array([0, -1 / np.sqrt(2), 0, 1 / np.sqrt(2)])

        return self.symOpCubic


def calcRot(vec1, vec2):
    """ Find the rotation matrix that aligns vec1 to vec2
    https://stackoverflow.com/questions/45142959/calculate-rotation-matrix-to-align-two-vectors-in-3d-space
    :param vec1: A 3d "source" vector
    :param vec2: A 3d "destination" vector
    :return mat: A transform matrix (3x3) which when applied to vec1, aligns it with vec2.
    """
    a, b = (vec1 / np.linalg.norm(vec1)).reshape(3), (vec2 / np.linalg.norm(vec2)).reshape(3)
    v = np.cross(a, b)
    c = np.dot(a, b)
    s = np.linalg.norm(v)
    kmat = np.array([[0, -v[2], v[1]], [v[2], 0, -v[0]], [-v[1], v[0], 0]])
    rot = np.eye(3) + kmat + kmat.dot(kmat) * ((1 - c) / (s ** 2))
    return rot


def random(dim=3):
    """
    Creating random orthogonal matrix, i.e. random rotation matrix
    https://stackoverflow.com/questions/38426349/how-to-create-random-orthonormal-matrix-in-python-numpy
    """
    random_state = np.random
    H = np.eye(dim)
    D = np.ones((dim,))
    for n in range(1, dim):
        x = random_state.normal(size=(dim - n + 1,))
        D[n - 1] = np.sign(x[0])
        x[0] -= D[n - 1] * np.sqrt((x * x).sum())
        # Householder transformation
        Hx = (np.eye(dim - n + 1) - 2. * np.outer(x, x) / (x * x).sum())
        mat = np.eye(dim)
        mat[n - 1:, n - 1:] = Hx
        H = np.dot(H, mat)
        # Fix the last sign such that the determinant is 1
    D[-1] = (-1) ** (1 - (dim % 2)) * D.prod()
    # Equivalent to np.dot(np.diag(D), H) but faster, apparently
    H = (D * H.T).T
    return H
